#include "sys.h"
#include "usart.h"
#include "delay.h"
#include "stm32f10x.h"
#include "stm32f10x_conf.h"
#include "timer.h"
#include "led.h"
#include "encoder.h"
#include "pid.h"

RCC_ClocksTypeDef get_rcc_clock;

int main(void) {
    u8 angle = 0;
    u8 speed = 0;
    delay_init();
    PWM_Init();
    Encoder_Init();


#if USART
    uart_init(115200);
#else
    SBUS_USART1_Config(100000);
#endif

    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
    pid_init(&pid);

    TIM1->CCR4 = 145;
	  RCC_GetClocksFreq(&get_rcc_clock);
    while (1) {
#if USART
        // ??
        // ??
        if (USART_RX_STA & 0x8000) {
            angle = (u8) (USART_RX_BUF[0] - '0');
//					  speed = (u8) (USART_RX_BUF[1] - '0');
						switch (angle){
							case 1:
								TIM1->CCR4 = 169;
								break;
							case 2:
								TIM1->CCR4 = 159;
								break;
							case 3:
								TIM1->CCR4 = 145;
								break;
							case 4:
								TIM1->CCR4 = 131;
								break;
							case 5:
								TIM1->CCR4 = 121;
							  break;
							default:
								TIM1->CCR4 = 145;
								break;
						}
            // ??
            pid.target = 2 * 10;

            USART_RX_STA = 0;
        }
        set_motor(pid_count(&pid));
#else
        // ??
        // ??
//        angle = (SBUS_Ch[2] - 200) / 32 + 120;
//        TIM1->CCR4 = angle;
//        // ??
//        speed = (SBUS_Ch[3] - 200) / 32;
//        if (SBUS_Ch[5] == 1000) {
//            pid_reset(&pid);
//            set_motor(0);
//        } else if (SBUS_Ch[5] == 200) {
//            pid.target = speed;
//        } else if (SBUS_Ch[5]) {
//            pid.target = -1 * speed;
//        }

//        pid.output = pid_count(&pid);
//        set_motor(pid.output);

//				// ??
//        		120 - 170 ???
//				if(SBUS_Ch[3] < 1000){
//						if(SBUS_Ch[6] == 200){
//								if(SBUS_Ch[5] == 200) TIM1->CCR4 = 121;
//								else if(SBUS_Ch[5] == 1000) TIM1->CCR4 = 129;
//								else if(SBUS_Ch[5] == 1800) TIM1->CCR4 = 137;
//						}
//						else if(SBUS_Ch[6] == 1800){
//								if(SBUS_Ch[5] == 200) TIM1->CCR4 = 145;
//				else if(SBUS_Ch[5] == 1000) TIM1->CCR4 = 153;
//				else if(SBUS_Ch[5] == 1800) TIM1->CCR4 = 161;
//						}
//				}
//
//				else{
//						TIM1->CCR4 = 169;
//				}

        if (SBUS_Ch[6] == 200) {
            if (SBUS_Ch[5] == 200) angle = 121;
            else if (SBUS_Ch[5] == 1000) angle = 145;
            else if (SBUS_Ch[5] == 1800) angle = 169;
        } else if (SBUS_Ch[6] == 1800) {
            if (SBUS_Ch[5] == 200) angle = 129;
            else if (SBUS_Ch[5] == 1000) angle = 145;
            else if (SBUS_Ch[5] == 1800) angle = 161;
        }
        TIM1->CCR4 = angle;

        speed = (SBUS_Ch[3] - 200) / 32;
        pid.target = speed;
				if(SBUS_Ch[2] < 500){
					pid.target = -1 * pid.target;
				}
        pid.output = pid_count(&pid);
        set_motor(pid.output);

#endif
        delay_ms(10);
    }

}







